package edu.wpi.first.wpilibj.swarm;

import edu.wpi.first.wpilibj.Timer;

public class Autonomous {
	private RobotMain robomain;
	
	Autonomous(RobotMain robotmain) {
		robomain = robotmain;
	}
	
	private boolean done() {
		if (robomain.isAutonomous() && robomain.isEnabled()) return false;
		return true;
	}
	
	private double sign(double x) {
		if (x > 0.0) return 1.0;
		if (x < 0.0) return -1.0;
		return 0.0;
	}
	
	private void move(double x, double y) {
		double  tgtx  = robomain.drivetrain.encoderfeedback.getX() + x;
		double  tgty  = robomain.drivetrain.encoderfeedback.getY() + y;
		double  xspd  = .2 * sign(x);
		double  yspd  = .3 * sign(y);
		boolean xdone = (xspd == 0.0);
		boolean ydone = (yspd == 0.0);
		
		robomain.drivetrain.setInput(xspd, yspd, 0.0, false);
		
		while (!(done() || (xdone && ydone))) {
			Timer.delay(0.1);
			
			if (xspd > 0.0 && robomain.drivetrain.encoderfeedback.getX() >= tgtx) {
				xspd = 0.0;
				robomain.drivetrain.setInput(xspd, yspd, 0.0, false);
				xdone = true;
			} else if (xspd < 0.0 && robomain.drivetrain.encoderfeedback.getX() <= tgtx) {
				xspd = 0.0;
				robomain.drivetrain.setInput(xspd, yspd, 0.0, false);
				xdone = true;
			}
			
			if (yspd > 0.0 && robomain.drivetrain.encoderfeedback.getY() >= tgty) {
				yspd = 0.0;
				robomain.drivetrain.setInput(xspd, yspd, 0.0, false);
				ydone = true;
			} else if (yspd < 0.0 && robomain.drivetrain.encoderfeedback.getY() <= tgty) {
				yspd = 0.0;
				robomain.drivetrain.setInput(xspd, yspd, 0.0, false);
				ydone = true;
			}
		}
	}
	
	private void wait(double t) {
		double delay;
		while (t > 0.01 && !done()) {
			delay = Math.min(t, 0.1);
			Timer.delay(delay);
			t    -= delay;
		}
	}
	
	private void kick() {
		robomain.kicker.setKicking(true);
		while (!done() && robomain.kicker.isKicking()) Timer.delay(0.1);
		robomain.kicker.setKicking(false);
	}
	
	private void path1() {
		move(-100.0, 0.0);
		if (done()) return;
		robomain.arm.setEnabled(true);
		robomain.arm.setTarget(0.7);
		wait(3.0);
		robomain.arm.setTarget(0.0);
		if (done()) return;
		move(10.0, 0.0);
		if (done()) return;
		move (0.0, -120.0);
	}
	
	private void path2() {
		move(0.0, -70.0);
		if (done()) return;
		move(-70.0, 0.0);
	}
	
	private void path3() {
		move(-90.0, 0.0);
		if (done()) return;
		move(0.0, 60.0);
		if (done()) return;
		move(-10.0, 0.0);
		robomain.arm.setEnabled(true);
		robomain.arm.setTarget(0.7);
		wait(3.0);
		robomain.arm.setTarget(0.0);
		if (done()) return;
		move(0.0, -120.0);
	}
	
	private void path4() {
		move(-100.0, 0.0);
		if (done()) return;
		robomain.arm.setEnabled(true);
		robomain.arm.setTarget(0.7);
		wait(3.0);
		robomain.arm.setTarget(0.0);
	}
	
	void continuous() {
		robomain.arm.setDisabledOut(0.0);
		robomain.arm.setEnabled(false);
		robomain.kicker.setEnabled(false);
		robomain.drivetrain.setInput(0.0, 0.0, 0.0, false);
		robomain.kicker.brushOff();
		robomain.drivetrain.resetGyro();
		robomain.kicker.setPos(0.2);
		if (robomain.teleop.io.getAutonomousMode() != 0) {
			robomain.kicker.setEnabled(true);
			kick();
		}
		if (done()) return;
		switch(robomain.teleop.io.getAutonomousMode()) {
			case 1:
				path1();
				break;
			case 2:
				path2();
				break;
			case 3:
				path3();
				break;
			case 4:
				path4();
				break;
		}
		robomain.drivetrain.setInput(0.0, 0.0, 0.0, false);
		while(!done()) Timer.delay(0.1);
	}
}